#include "cpp_unit_lite/TestHarness.hpp"
#include "common/kalman.h"

using namespace common;

TEST(Kalman, Simple)
{
	KalmanFilter filter;

	// Инициализация
	filter.setMeasureUncertainity(Matrix::Identity());
	filter.setInitialState(
		RectCoords(0.0, 0.0), 
		RectCoords(1.0, 1.0), 
		Matrix::Identity(),
		0.0);

	// Имитируем линейное движение; скорость изначально задали
	// правильную
	for (int i = 1; i < 10; ++i)
	{
		filter.updateState(RectCoords(i, i), i);
		RectCoords vel = filter.getVel(), coords = filter.getCoords();
		DOUBLES_EQUAL(i, coords.x, UTEST_THRESHOLD);
		DOUBLES_EQUAL(i, coords.y, UTEST_THRESHOLD);
		DOUBLES_EQUAL(1.0, vel.x, UTEST_THRESHOLD);
		DOUBLES_EQUAL(1.0, vel.y, UTEST_THRESHOLD);
	}
}